#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Author  : Lee
# @File    : analysis_record_bag.py
# @Time    : 2024/2/20 13:31
import os
import csv
import math
from cyber_record.record import Record


def record_to_csv(record_path=r"."):
    for record_name in os.listdir(record_path):
        if ".record." in record_name:
            try:
                _record_position_list = list()
                _record = Record(os.path.join(record_path, record_name))
                for channel_name, data, timestamp in _record.read_messages():
                    # 经纬度 "/Localization/GnssBestPose"
                    # 地图坐标系 "/Localization/LocalizationEstimate"
                    # if channel_name == "/Planning/ADCTrajectory":
                    #
                    #     print(data.trajectory_points)
                    if channel_name == "/Localization/LocalizationEstimate":
                        _record_position_list.append([
                            data.header.timestamp_sec, data.pose.position.x, data.pose.position.y,
                            math.sqrt(data.pose.linear_velocity.x**2 + data.pose.linear_velocity.y**2),
                            data.pose.heading])

                with open(f"{record_path}/{record_name.split('.')[0]}.csv", 'w', newline='') as file:
                    writer = csv.writer(file)
                    writer.writerows(_record_position_list)
            except Exception as e:
                print(f"{record_name} 解析失败")


if __name__ == '__main__':
    record_to_csv("../cyber_files")
